// modbus_tcp_master.c
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <sys/socket.h>

#define SERVER_IP "127.0.0.1"
#define SERVER_PORT 502

// 辅助函数：创建Modbus TCP连接
int create_modbus_connection() {
    int sock = socket(AF_INET, SOCK_STREAM, 0);
    if (sock < 0) {
        perror("Socket creation failed");
        return -1;
    }

    struct sockaddr_in serv_addr;
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(SERVER_PORT);
    
    if (inet_pton(AF_INET, SERVER_IP, &serv_addr.sin_addr) <= 0) {
        perror("Invalid address/Address not supported");
        close(sock);
        return -1;
    }

    if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) {
        perror("Connection Failed");
        close(sock);
        return -1;
    }

    return sock;
}

// 功能码0x01: 读线圈状态
void modbus_read_coils(uint8_t unit_id, uint16_t start_addr, uint16_t quantity, uint8_t *coil_status) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t request[12] = {
        0x00, 0x01,             // Transaction ID
        0x00, 0x00,             // Protocol ID
        0x00, 0x06,             // Length
        unit_id,                // Unit ID
        0x01,                   // Function Code (Read Coils)
        (start_addr >> 8) & 0xFF, start_addr & 0xFF,
        (quantity >> 8) & 0xFF, quantity & 0xFF
    };

    send(sock, request, sizeof(request), 0);

    uint8_t response[256];
    ssize_t bytes_received = recv(sock, response, sizeof(response), 0);

    if (bytes_received > 0 && response[7] == 0x01) {
        uint8_t byte_count = response[8];
        memcpy(coil_status, &response[9], byte_count);
    }

    close(sock);
}

// 功能码0x02: 读离散输入
void modbus_read_discrete_inputs(uint8_t unit_id, uint16_t start_addr, uint16_t quantity, uint8_t *input_status) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t request[12] = {
        0x00, 0x01,             // Transaction ID
        0x00, 0x00,             // Protocol ID
        0x00, 0x06,             // Length
        unit_id,                // Unit ID
        0x02,                   // Function Code (Read Discrete Inputs)
        (start_addr >> 8) & 0xFF, start_addr & 0xFF,
        (quantity >> 8) & 0xFF, quantity & 0xFF
    };

    send(sock, request, sizeof(request), 0);

    uint8_t response[256];
    ssize_t bytes_received = recv(sock, response, sizeof(response), 0);

    if (bytes_received > 0 && response[7] == 0x02) {
        uint8_t byte_count = response[8];
        memcpy(input_status, &response[9], byte_count);
    }

    close(sock);
}

// 功能码0x03: 读保持寄存器
void modbus_read_holding_registers(uint8_t unit_id, uint16_t start_addr, uint16_t quantity, uint16_t *values) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t request[12] = {
        0x00, 0x01,             // Transaction ID
        0x00, 0x00,             // Protocol ID
        0x00, 0x06,             // Length
        unit_id,                // Unit ID
        0x03,                   // Function Code (Read Holding Registers)
        (start_addr >> 8) & 0xFF, start_addr & 0xFF,
        (quantity >> 8) & 0xFF, quantity & 0xFF
    };

    send(sock, request, sizeof(request), 0);

    uint8_t response[256];
    ssize_t bytes_received = recv(sock, response, sizeof(response), 0);

    if (bytes_received > 0 && response[7] == 0x03) {
        uint8_t byte_count = response[8];
        for (int i = 0; i < byte_count/2; i++) {
            values[i] = (response[9 + i*2] << 8) | response[10 + i*2];
        }
    }

    close(sock);
}

// 功能码0x04: 读输入寄存器
void modbus_read_input_registers(uint8_t unit_id, uint16_t start_addr, uint16_t quantity, uint16_t *values) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t request[12] = {
        0x00, 0x01,             // Transaction ID
        0x00, 0x00,             // Protocol ID
        0x00, 0x06,             // Length
        unit_id,                // Unit ID
        0x04,                   // Function Code (Read Input Registers)
        (start_addr >> 8) & 0xFF, start_addr & 0xFF,
        (quantity >> 8) & 0xFF, quantity & 0xFF
    };

    send(sock, request, sizeof(request), 0);

    uint8_t response[256];
    ssize_t bytes_received = recv(sock, response, sizeof(response), 0);

    if (bytes_received > 0 && response[7] == 0x04) {
        uint8_t byte_count = response[8];
        for (int i = 0; i < byte_count/2; i++) {
            values[i] = (response[9 + i*2] << 8) | response[10 + i*2];
        }
    }

    close(sock);
}

// 功能码0x05: 写单个线圈
void modbus_write_single_coil(uint8_t unit_id, uint16_t address, uint8_t value) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t request[12] = {
        0x00, 0x01,             // Transaction ID
        0x00, 0x00,             // Protocol ID
        0x00, 0x06,             // Length
        unit_id,                // Unit ID
        0x05,                   // Function Code (Write Single Coil)
        (address >> 8) & 0xFF, address & 0xFF,
        value ? 0xFF : 0x00,    // On/Off value
        0x00                    // Padding
    };

    send(sock, request, sizeof(request), 0);
    close(sock);
}

// 功能码0x06: 写单个寄存器
void modbus_write_single_register(uint8_t unit_id, uint16_t address, uint16_t value) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t request[12] = {
        0x00, 0x01,             // Transaction ID
        0x00, 0x00,             // Protocol ID
        0x00, 0x06,             // Length
        unit_id,                // Unit ID
        0x06,                   // Function Code (Write Single Register)
        (address >> 8) & 0xFF, address & 0xFF,
        (value >> 8) & 0xFF, value & 0xFF
    };

    send(sock, request, sizeof(request), 0);
    close(sock);
}

// 功能码0x0F: 写多个线圈
void modbus_write_multiple_coils(uint8_t unit_id, uint16_t start_addr, uint16_t quantity, uint8_t *coil_values) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t byte_count = (quantity + 7) / 8;
    uint8_t request[13 + byte_count];
    
    request[0] = 0x00;  // Transaction ID MSB
    request[1] = 0x01;  // Transaction ID LSB
    request[2] = 0x00;  // Protocol ID MSB
    request[3] = 0x00;  // Protocol ID LSB
    request[4] = 0x00;  // Length MSB (will be set later)
    request[5] = 0x00;  // Length LSB (will be set later)
    request[6] = unit_id;
    request[7] = 0x0F;  // Function Code (Write Multiple Coils)
    request[8] = (start_addr >> 8) & 0xFF;
    request[9] = start_addr & 0xFF;
    request[10] = (quantity >> 8) & 0xFF;
    request[11] = quantity & 0xFF;
    request[12] = byte_count;
    
    memcpy(&request[13], coil_values, byte_count);
    
    // Set length field
    uint16_t length = 7 + byte_count;
    request[4] = (length >> 8) & 0xFF;
    request[5] = length & 0xFF;

    send(sock, request, 13 + byte_count, 0);
    close(sock);
}

// 功能码0x10: 写多个寄存器
void modbus_write_multiple_registers(uint8_t unit_id, uint16_t start_addr, uint16_t quantity, uint16_t *values) {
    int sock = create_modbus_connection();
    if (sock < 0) return;

    uint8_t byte_count = quantity * 2;
    uint8_t request[13 + byte_count];
    
    request[0] = 0x00;  // Transaction ID MSB
    request[1] = 0x01;  // Transaction ID LSB
    request[2] = 0x00;  // Protocol ID MSB
    request[3] = 0x00;  // Protocol ID LSB
    request[4] = 0x00;  // Length MSB (will be set later)
    request[5] = 0x00;  // Length LSB (will be set later)
    request[6] = unit_id;
    request[7] = 0x10;  // Function Code (Write Multiple Registers)
    request[8] = (start_addr >> 8) & 0xFF;
    request[9] = start_addr & 0xFF;
    request[10] = (quantity >> 8) & 0xFF;
    request[11] = quantity & 0xFF;
    request[12] = byte_count;
    
    for (int i = 0; i < quantity; i++) {
        request[13 + i*2] = (values[i] >> 8) & 0xFF;
        request[14 + i*2] = values[i] & 0xFF;
    }
    
    // Set length field
    uint16_t length = 7 + byte_count;
    request[4] = (length >> 8) & 0xFF;
    request[5] = length & 0xFF;

    send(sock, request, 13 + byte_count, 0);
    close(sock);
}